/**
* @file PoseData.h
* @author
* @brief 位姿数据类型
* @version 1.0
* @date 2020-07-23
*
* @copyright Copyright (c) 2020
*
*/

#pragma once

#include "data_struct/geometry/Point.h"
#include "data_struct/geometry/PointCloud.h"

namespace behavior_controller
{
class PoseF : public PointF
{
public:
    typedef Eigen::Rotation2D<double> Orientation;

    PoseF() {}

    PoseF(const double &x, const double &y, const double &angle)
            : PointF(x, y), Orientation_(angle) {}

    PoseF(const Point<double> &pt, const double &angle)
            : PointF(pt), Orientation_(angle) {}

    inline double angle() const
    { return Orientation_.angle(); };

    /**
     * @brief 逆矩阵，用于坐标系转换
     * @return
     */
    PoseF Inversion() const;

    /**
     * @brief 根据提供的点 进行矩阵变换运算
     * @return
     */
    PointF Convert(const PointF &pt) const;

    /**
     * @brief 根据提供的pose 进行矩阵变换运算
     * @return
     */
    PoseF Convert(const PoseF &pose) const;

    /**
     * @brief 将当前位姿的坐标，转换为 提供Pose 所在坐标系下的坐标
     * @param 提供Pose
     * @return 提供Pose所在坐标系下的坐标
     */
    PoseF ConvertOnCur(const PoseF &pose) const;

    /**
     * @brief 将括号内提供的Pose 转换为以 当前位姿坐标为原点坐标系的坐标
     * @param 提供Pose
     * @return 提供Pose为原点坐标系下的坐标
     */
    PoseF ConvertOriginCur(const PoseF &pose) const;

    /**
     * @brief 将当前位姿的坐标，转换为 提供点 所在坐标系下的坐标
     * @param 提供点
     * @return 提供点所在坐标系下的坐标
     */
    PointF ConvertOnCur(const PointF &point) const;

    /**
     * @brief  将括号内提供的点 转换为以 当前位姿坐标为原点坐标系的坐标
     * @param 提供点
     * @return 提供点为原点坐标系下的坐标
     */
    PointF ConvertOriginCur(const PointF &point) const;

    /**
     * @brief  将当前点云转换为机器人坐标系
     * @param 提供点云
     * @return 机器人坐标系点云
     */
    PointCloud<double> ConvertToLocalCloud(PointCloud<double> &point_cloud) const;

    /**
     * @brief  将当前点云转换为世界坐标系
     * @param 提供点云
     * @return 世界坐标系点云
     */
    PointCloud<double> ConvertToWorldCloud(PointCloud<double> &point_cloud) const;

    Orientation Orientation_; //方向角
};
} // namespace behavior_controller